/*
 * main.cpp
 *
 *  Created on: May 29, 2009
 *      Author: flaviotruzzi
 */


#include <stdlib.h>
#include <stdio.h>
#include <cv.h>
#include <highgui.h>
#include "LtiBandPass.h"
#include "LtiCam.h"
#include "Aux.h"
#include "KTracker.h"
#include <iostream>
#include <libplayerc++/playerc++.h>
/*
 *  argv[1] - arquivo ou numero da camera
 * argv[2] - distancia mínima
 * argv[3] - distancia mínima máxima
 *
 */


int main(int argc, char** argv) {
	double mindist=0.5f;
	double mmindist=0.3f;
	if (argc != 3)
		std::cout << "Usando parametros padrao" << std::endl;
	else {
		double mindist = atof(argv[2]);
		double mmindist = atof(argv[3]);
	}

	int start=0;
	double sonardist;
	double speed = 0.0f;
	double turnrate = 0.0f;

	int ymin = 0;
	int ymax = 57;
	int cbmin = 111;
	int cbmax = 128;
	int crmin = 143;
	int crmax = 255;
	int x = 0;
	int y = 0;

    LtiCam camera(argv[1]);
	IplImage* img = cvCreateImage(cvGetSize(camera.frame),8,1);
    LtiBandPass Filtro(camera.frame);
    KTracker Tracker;
    Aux Functions;
    Filtro.setBand(ymin,ymax,cbmin,cbmax,crmin,crmax);
    cvNamedWindow("teste", -1);
    cvNamedWindow("threshold", -1);
    cvCreateTrackbar( "Ymin", "threshold", &ymin, 255, 0 );
    cvCreateTrackbar( "Ymax", "threshold", &ymax, 255, 0 );
    cvCreateTrackbar( "Cbmin", "threshold", &cbmin, 255, 0 );
    cvCreateTrackbar( "Cbmax", "threshold", &cbmax, 255, 0 );
    cvCreateTrackbar( "Crmin", "threshold", &crmin, 255, 0 );
    cvCreateTrackbar( "Crmax", "threshold", &crmax, 255, 0 );
    Filtro.Read(camera.frame);

    try {
    	using namespace PlayerCc;

        PlayerClient robot ("localhost",6665);
        Position2dProxy pp (&robot, 0);
        SonarProxy sp      (&robot, 0);

        std::cout << robot << std::endl;

        pp.SetMotorEnable (true);

        bool needtomovef = true;
        bool needtorotate = true;

        Tracker.getContour(Filtro.thresholded);
        Tracker.getOInfo();
        double distance = Tracker.getDistance();
        double radius = Tracker.getW();
        while (camera.frame != NULL) {
        	camera.Read();
        	Filtro.Read(camera.frame);
        	Filtro.setBand(ymin,ymax,cbmin,cbmax,crmin,crmax);
        	Filtro.threshold(img);
        	Filtro.ApplyMorph();
         	cvShowImage("threshold",Filtro.thresholded);
        	Tracker.predict();
         	Tracker.getContour(Filtro.thresholded);
         	Tracker.getOInfo();
         	Functions.DrawCross(camera.workimg,Tracker.massCenter,CV_RGB(255,0,0));
         	Functions.DrawCross(camera.workimg, Tracker.massFilter,CV_RGB(0,0,255) );
         	cvShowImage("teste",camera.workimg);
         	Tracker.correct();

         	distance = Tracker.getDistance();
         	radius = Tracker.getW();
         	/* ROBO */
         	robot.Read();
         	speed = 0.3;
         	turnrate = 0.2;

         	// mindist = distancia minima superior do alvo
         	// mmindist = distancia minima inferior do alvo
         	if (distance > mindist) {
         		if ((sp[2] < mmindist) ||
         				(sp[3] < mmindist) ||
         				(sp[4] < mmindist) ||
         				(sp[5] < mmindist)) {
         			needtomovef = true;
         			std::cout << "infrente" << std::endl;
         			speed = -0.1f;
         		}
         		else if((sp[2] < mindist) ||
         				(sp[3] < mindist) ||
         				(sp[3] < mindist) ||
         				(sp[4] < mindist) ||
         				(sp[5] < mindist)) {
         			needtomovef = true;
         			std::cout << "vorta " << std::endl;
         			speed = 0.1f;
         		}
         	}
            else
         	needtomovef = false;

               if (radius > 0)
         	{
         	  needtorotate = true;
         	  turnrate = 0.1;
         	}
               else if (radius < 0)
         	{
         	  needtorotate = true;
         	  turnrate = -0.1;
         	}
               else
            	   needtorotate = false;
               if ((needtomovef) && (needtorotate))
            	   pp.SetSpeed(speed,turnrate);
               else if (needtomovef)
            	   pp.SetSpeed(speed,0);
               else if (needtorotate)
            	   pp.SetSpeed(0,turnrate);
               else
            	   pp.SetSpeed(0,0);

               /*********************************/

        	printf("Distancia %6g    rotate: %6g\n",Tracker.getDistance(),Tracker.getW());
        	int c = cvWaitKey(10);
        	if (char(c) == 27)
        		break;
         }




    } catch (PlayerCc::PlayerError e) {
    	std::cerr << e << std::endl;
    	return -1;
    }



    Tracker.~KTracker();
    Filtro.~LtiBandPass();
    camera.~LtiCam();


    return (EXIT_SUCCESS);
}
